#pragma once

#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_extrapolator.h"

#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/map_builder_bridge.h"
#include "cartographer_ros/metrics/family_factory.h"
#include "cartographer_ros/node_constants.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/trajectory_options.h"

#include "nav_msgs/msg/odometry.hpp"
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

using namespace cartographer;

namespace cartographer_ros
{
    class TrajectoryPublisher
    {
    public:
        TrajectoryPublisher(rclcpp::Node::SharedPtr &node, const cartographer_ros::NodeOptions &node_options);

        ~TrajectoryPublisher()
        {
        }

        void Publish(const MapBuilderBridge::LocalTrajectoryData &trajectory_data,
                     mapping::PoseExtrapolator &extrapolator, int traj_id);

    private:
        rclcpp::Node::SharedPtr node_;
        const cartographer_ros::NodeOptions &node_options_;

        std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

        std::map<int, builtin_interfaces::msg::Time> last_published_tf_stamps_;

        ::rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scan_matched_publisher_;
        ::rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr tracked_pose_publisher_;
    };
} // namespace cartographer_ros